package com.robotic.tcp;

import lombok.extern.slf4j.Slf4j;
import org.springframework.stereotype.Service;

import javax.annotation.PostConstruct;
import javax.annotation.PreDestroy;
import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;
import java.net.InetSocketAddress;
import java.net.Socket;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.nio.charset.StandardCharsets;
import java.time.LocalDateTime;
import java.time.format.DateTimeFormatter;
import java.util.concurrent.atomic.AtomicInteger;
import java.util.concurrent.locks.ReentrantLock;
import java.util.regex.Matcher;
import java.util.regex.Pattern;

/**
 * 改进版 RobotCommunicationService
 * 特性：
 *  - 线程安全
 *  - 带指数退避的自动重连
 *  - 防止重复连接冲突
 *  - 保留原业务逻辑
 */
@Slf4j
@Service
public class RobotCommunicationService {

    private static final String ROBOT_HOST = "10.110.7.219";
    private static final int ROBOT_PORT = 30000;
    private static final int DEFAULT_POLL_TIMEOUT_MS = 5000;

    // 报文ID计数器
    private static final AtomicInteger messageIdCounter = new AtomicInteger(0);

    private Socket robotSocket;
    private DataOutputStream out;
    private DataInputStream in;

    private final ReentrantLock socketLock = new ReentrantLock();

    // 指数退避参数
    private long reconnectDelay = 2000; // 初始2秒
    private static final long MAX_RECONNECT_DELAY = 30000; // 最大30秒

    @PostConstruct
    public void initialize() {
        connectSafe();
    }

    @PreDestroy
    public void cleanup() {
        closeConnection();
    }

    /**
     * 安全连接（线程安全、受锁保护）
     */
    private void connectSafe() {
        socketLock.lock();
        try {
            if (isConnected()) {
                return;
            }

            log.info("正在尝试连接到机器人 {}:{}...", ROBOT_HOST, ROBOT_PORT);
            closeConnection();

            Socket socket = new Socket();
            socket.connect(new InetSocketAddress(ROBOT_HOST, ROBOT_PORT), 10000); // 10秒超时
            socket.setSoTimeout(DEFAULT_POLL_TIMEOUT_MS);

            out = new DataOutputStream(socket.getOutputStream());
            in = new DataInputStream(socket.getInputStream());
            robotSocket = socket;

            log.info("成功连接到机器人。");
            reconnectDelay = 2000; // 重置退避
        } catch (IOException e) {
            log.warn("连接机器人失败: {}", e.getMessage());
            closeConnection();
        } finally {
            socketLock.unlock();
        }
    }

    /**
     * 指数退避自动重连
     */
    private void reconnectWithBackoff() {
        while (!isConnected()) {
            connectSafe();
            if (isConnected()) {
                return;
            }
            try {
                log.warn("连接失败，等待 {} ms 后重试...", reconnectDelay);
                Thread.sleep(reconnectDelay);
                reconnectDelay = Math.min(reconnectDelay * 2, MAX_RECONNECT_DELAY);
            } catch (InterruptedException ignored) {
            }
        }
    }

    /**
     * 判断连接是否有效
     */
    public boolean isConnected() {
        return robotSocket != null
                && robotSocket.isConnected()
                && !robotSocket.isClosed()
                && robotSocket.isBound();
    }

    /**
     * 关闭连接
     */
    private void closeConnection() {
        try {
            if (in != null) in.close();
            if (out != null) out.close();
            if (robotSocket != null) robotSocket.close();
        } catch (IOException e) {
            log.warn("关闭旧连接时发生错误: {}", e.getMessage());
        } finally {
            in = null;
            out = null;
            robotSocket = null;
        }
    }

    /**
     * 主功能：获取机器人状态
     */
    public RobotState fetchRobotState(String requestType) {
        socketLock.lock();
        try {
            if (!isConnected()) {
                log.warn("连接已断开，尝试重新连接...");
                reconnectWithBackoff();
                if (!isConnected()) {
                    log.error("重连失败，跳过本次请求 (类型: {})", requestType);
                    return null;
                }
            }

            int messageId = messageIdCounter.getAndUpdate(n -> (n + 1) % 65536);
            byte[] requestPacket = buildRequestPacket(requestType, messageId);

            out.write(requestPacket);
            out.flush();

            byte[] responseHeader = new byte[16];
            in.readFully(responseHeader);
            ByteBuffer headerBuffer = ByteBuffer.wrap(responseHeader).order(ByteOrder.LITTLE_ENDIAN);

            int responseMessageId = headerBuffer.getShort(6) & 0xFFFF;
            if (responseMessageId != messageId) {
                log.error("收到ID不匹配的响应。请求ID={}, 响应ID={}", messageId, responseMessageId);
                return null;
            }

            int asduLength = headerBuffer.getShort(4) & 0xFFFF;
            if (asduLength <= 0) {
                log.error("接收到无效数据长度: {}", asduLength);
                return null;
            }

            byte[] asduData = new byte[asduLength];
            in.readFully(asduData);
            String responseXml = new String(asduData, StandardCharsets.UTF_8);
            return parseResponseXml(requestType, responseXml);

        } catch (IOException e) {
            log.error("通信异常 (类型: {}): {}", requestType, e.getMessage());
            closeConnection();
            return null;
        } catch (Exception e) {
            log.error("未知异常 (类型: {}): {}", requestType, e.getMessage(), e);
            closeConnection();
            return null;
        } finally {
            socketLock.unlock();
        }
    }

    /**
     * XML解析
     */
    private RobotState parseResponseXml(String requestType, String xml) {
        RobotState state = new RobotState();
        if ("1002".equals(requestType)) {
            parseBaseStatus(xml, state);
        } else if ("2001".equals(requestType)) {
            parseBatteryInfo(xml, state);
        }
        return state;
    }

    private void parseBaseStatus(String xml, RobotState state) {
        state.setMotionState(getMotionStateString(extractValue(xml, "MotionState")));
        state.setGaitState(extractValue(xml, "GaitState"));
        state.setPosX(parseDouble(extractValue(xml, "PosX")));
        state.setPosY(parseDouble(extractValue(xml, "PosY")));
        state.setPosZ(parseDouble(extractValue(xml, "PosZ")));
        state.setSpeed(extractValue(xml, "Speed"));
        state.setSumOdom(extractValue(xml, "SumOdom"));
        state.setElectricity(extractValue(xml, "Electricity"));
    }

    private void parseBatteryInfo(String xml, RobotState state) {
        state.setVoltage(extractValue(xml, "Voltage"));
        String currentRaw = extractValue(xml, "Current");
        if (currentRaw != null) {
            try {
                double currentA = Integer.parseInt(currentRaw) * 0.01;
                state.setCurrent(String.format("%.2f", currentA));
            } catch (NumberFormatException ignored) {
            }
        }
    }

    private byte[] buildRequestPacket(String type, int msgId) {
        String timestamp = LocalDateTime.now().format(DateTimeFormatter.ofPattern("yyyy-MM-dd HH:mm:ss"));
        String asduXml = String.format(
                "<?xml version=\"1.0\" encoding=\"UTF-8\"?><PatrolDevice><Type>%s</Type><Command>1</Command><Time>%s</Time><Items/></PatrolDevice>",
                type, timestamp
        );

        byte[] asduBytes = asduXml.getBytes(StandardCharsets.UTF_8);
        ByteBuffer header = ByteBuffer.allocate(16).order(ByteOrder.LITTLE_ENDIAN);
        header.put(new byte[]{(byte) 0xeb, (byte) 0x90, (byte) 0xeb, (byte) 0x90});
        header.putShort((short) asduBytes.length);
        header.putShort((short) msgId);
        header.put(new byte[8]);

        ByteBuffer finalPacket = ByteBuffer.allocate(16 + asduBytes.length);
        finalPacket.put(header.array());
        finalPacket.put(asduBytes);
        return finalPacket.array();
    }

    private String extractValue(String xml, String tagName) {
        Matcher m = Pattern.compile("<" + tagName + ">(.*?)</" + tagName + ">").matcher(xml);
        return m.find() ? m.group(1) : null;
    }

    private Double parseDouble(String value) {
        if (value == null) return null;
        try {
            return Double.parseDouble(value);
        } catch (NumberFormatException e) {
            return null;
        }
    }

    private String getMotionStateString(String code) {
        if (code == null) return "N/A";
        switch (code) {
            case "0": return "趴下";
            case "1": return "正在起立";
            case "2": return "初始站立";
            case "3": return "力控站立";
            case "4": return "踏步";
            case "5": return "正在趴下";
            case "6": return "软急停";
            case "7": return "摔倒";
            case "16": return "RL状态";
            default: return code;
        }
    }
}
